#ifndef SR300C_HPP
#define SR300C_HPP

#include "sr300b.hpp"

class SR300C : public SR300B
{
public:
    std::vector<cv::Vec6d> crossPos;
public:
    SR300C()
        : SR300B()
    {

    }

    virtual void ProcessFrame(const cv::Mat &color, const cv::Mat &ir, const cv::Mat &depth, const cv::Mat &xyz, const cv::Mat &texture, const cv::Mat &aligned_depth, const cv::Mat &aligned_xyz)
    {
        SR300::ProcessFrame(color, ir, depth, xyz, texture, aligned_depth, aligned_xyz);

        char ipUR[]="192.168.1.13";
        char ipPC[]="192.168.1.14";

        cv::Vec6d p;
        URControl::GetTCPPose(ipUR,ipPC, p[0],p[1],p[2],p[3],p[4],p[5]);
        printf("[%g,%g,%g,%g,%g,%g]\n",p[0],p[1],p[2],p[3],p[4],p[5]);

        std::vector<cv::Vec6d> crossPos1;
        CrossPosGlobal(crossPos1, xyz, p);

        crossPos.insert(crossPos.end(),crossPos1.begin(),crossPos1.end());
    }


    static void GetSequence(std::vector<cv::Vec2i> &xy, int nx, int ny)
    {
        for(int i=0;i<nx;i++)
        {
            if(i%2)
            {
                for(int j=ny-1;j>=0;j--)
                    xy.push_back(cv::Vec2i(i,j));
            }
            else
            {
                for(int j=0;j<ny;j++)
                    xy.push_back(cv::Vec2i(i,j));
            }
        }
    }

    static void GetSequencePos(std::vector<cv::Vec6d> &scanPos, cv::Vec6d startPos, cv::Size2d scanArea, cv::Size2d scanStep=cv::Size2d(0.15,0.15))
    {
        int nx=ceil(scanArea.width/scanStep.width);
        int ny=ceil(scanArea.height/scanStep.height);

        cv::Size2d scanStep1(scanArea.width/nx, scanArea.height/ny);

        nx++;
        ny++;

        std::vector<cv::Vec2i> xy;
        GetSequence(xy,nx,ny);

        cv::Matx22d rm(cos(startPos[5]), -sin(startPos[5]), sin(startPos[5]), cos(startPos[5]));


        for(auto xy1:xy)
        {
            cv::Vec2d pos=rm*cv::Vec2d(xy1[0]*scanStep1.width, xy1[1]*scanStep1.height);
            scanPos.push_back(cv::Vec6d(pos[0],pos[1],0,0,0,0)+startPos);
        }

    }



    static void UniquePos(std::vector<cv::Vec6d> &pos, double distanceThreshold=0.08)
    {
        std::vector<cv::Vec6d> posu;
        std::vector<size_t> count;

        for(auto p1:pos)
        {
            bool found=false;

            for(size_t i=0;i<posu.size();i++)
            {
                double d=cv::norm(posu[i]-p1);
                if(d<distanceThreshold)
                {
                    posu[i]=(posu[i]*(double)count[i]+p1)/(double)(count[i]+1);
                    count[i]++;
                    found=true;
                    break;
                }
            }

            if(!found)
            {
                posu.push_back(p1);
                count.push_back(1);
            }
        }

        pos.assign(posu.begin(),posu.end());
    }


    static void ProjectToXY(std::vector<cv::Vec2d> &xy, const std::vector<cv::Vec6d> &p, double rad)
    {
        cv::Matx22d rm(cos(rad), sin(rad), -sin(rad), cos(rad));
        for(auto p1:p)
        {
            xy.push_back(rm*cv::Vec2d(p1[0],p1[1]));
        }

        cv::Vec2d minxy=xy[0];

        for(auto xy1:xy)
        {
            if(minxy[0]>xy1[0])
                minxy[0]=xy1[0];
            if(minxy[1]>xy1[1])
                minxy[1]=xy1[1];
        }

        for(auto &xy1:xy)
        {
            xy1-=minxy;
        }

     }



    static void Sort(std::vector<cv::Vec6d> &p, double rad)
    {
        std::vector<cv::Vec2d> xy;
        ProjectToXY(xy, p, rad);

        std::cout<<cv::Mat(xy)<<"\n";

        std::vector<cv::Vec2i> ind;

        for(auto &xy1:xy)
        {
            cv::Vec2d pii(round(xy1[0]/0.15), round(xy1[1]/0.15));
//            if(pii[0]<4 && pii[1]<4)
            ind.push_back(pii);

        }

        std::cout<<cv::Mat(ind)<<"\n";

        cv::Vec2i maxind=ind[0];
        for(auto ind1:ind)
        {
            if(maxind[0]<ind1[0])
                maxind[0]=ind1[0];
            if(maxind[1]<ind1[1])
                maxind[1]=ind1[1];
        }


        std::vector<int> index;
        for(auto ind1:ind)
        {
            index.push_back(ind1[1]*(maxind[0]+1)+ind1[0]);
        }

        int maxindex=*std::max_element(index.begin(),index.end());

         std::cout<<cv::Mat(index)<<"\n";

         std::vector<cv::Vec6d> pu;
         for(int i=0;i<=maxindex;i++)
         {
             std::vector<int>::iterator it=std::find(index.begin(),index.end(), i);
             if(it!=index.end())
             {
                 int iindex=it-index.begin();
                 if(ind[iindex][0]<4 && ind[iindex][1]<4)
                 pu.push_back(p[iindex]);
             }
         }

         std::cout<<cv::Mat(pu)<<"\n";

          p.assign(pu.begin(),pu.end());
    }


    void Scan(cv::Size2d scanArea)
    {
        char ipUR[]="192.168.1.13";
        char ipPC[]="192.168.1.14";

        cv::Vec6d p(0.405317,-0.474087,0.450027,0,0,1.86385);


        std::vector<cv::Vec6d> scanPos;

        GetSequencePos(scanPos, p, scanArea);

        for(auto sp:scanPos)
        {

            std::cout<<sp<<"\n"<<std::flush;
            URControl::SetTCPPose(ipUR,ipPC, sp[0],sp[1],sp[2],sp[3],sp[4],sp[5]);

            std::cout<<"end\n"<<std::flush;

            sleep(1);

            for(int j=0;j<5;j++)
            GetOne();

        }

        URControl::SetTCPPose(ipUR,ipPC, p[0],p[1],p[2],p[3],p[4],p[5]);

        UniquePos(crossPos);

        std::cout<<"crossPos="<<crossPos.size()<<"\n"<<std::flush;

        Sort(crossPos, p[5]);

#if 0
        std::cout<<cv::Mat(crossPos)<<"\n"<<std::flush;
#else

        for(auto sp:crossPos)
        {

            std::cout<<sp<<"\n"<<std::flush;
            URControl::SetTCPPose(ipUR,ipPC, sp[0],sp[1],sp[2]-0.02,sp[3],sp[4],sp[5]);

            URControl::SetTCPPose(ipUR,ipPC, sp[0],sp[1],sp[2],sp[3],sp[4],sp[5]);

            URControl::SetTCPPose(ipUR,ipPC, sp[0],sp[1],sp[2]-0.02,sp[3],sp[4],sp[5]);
            std::cout<<"end\n"<<std::flush;

            sleep(2);
        }
#endif

    }



};


#endif
